/*
 * eyes_handler.cpp
 *
 *  Created on: Jan 24, 2013
 *      Author: michiel
 */

#include "emotion/eyes_handler.h"

EyesHandler::EyesHandler() {
	_eyes_left_pub = nh_.advertise<std_msgs::Float64>(Topic::EYES_LEFT_YAW, 1);
	_eyes_right_pub = nh_.advertise<std_msgs::Float64>(Topic::EYES_RIGHT_YAW, 1);
	_eyes_pitch_pub = nh_.advertise<std_msgs::Float64>(Topic::EYES_PITCH, 1);

	std_msgs::Float64 speed;
	speed.data = 0.01;

	ros::Publisher ps = nh_.advertise<std_msgs::Float64>("/tilt_controller/set_speed", 10);
	ros::Publisher ls = nh_.advertise<std_msgs::Float64>("/left_controller/set_speed", 10);
	ros::Publisher rs = nh_.advertise<std_msgs::Float64>("/right_controller/set_speed", 10);

	ps.publish(speed);
	ls.publish(speed);
	rs.publish(speed);
}

EyesHandler::~EyesHandler() {

}

void EyesHandler::leftEyeCallback(const std_msgs::Float64::ConstPtr &msg) {

}

void EyesHandler::rightEyeCallback(const std_msgs::Float64::ConstPtr &msg) {

}

void EyesHandler::pitchEyeCallback(const std_msgs::Float64::ConstPtr &msg) {

}

void EyesHandler::init() {

}

void EyesHandler::spin() {
	ros::Rate loop_rate(30);

	while (ros::ok()) {
		if (emoteOk()) {
			processEmotion();
			playEmotion();
		}

		ros::spinOnce();
		loop_rate.sleep();
	}

}

void EyesHandler::processEmotion() {
	if (getEmotion() != _prev_state) {
		switch (getEmotion()) {
		case EMOTION_HAPPY:
			setEyesState(EYES_SPIN);
			_message_sent = false;
			break;
		case EMOTION_BORED:
			setEyesState(EYES_NEUTRAL);
			_message_sent = false;
			break;
		case EMOTION_SAD:
			setEyesState(EYES_DOWN);
			_message_sent = false;
			break;
		default:
			setEyesState(EYES_NEUTRAL);
			_message_sent = false;
			break;
		}
		_prev_state = getEmotion();
	}
}

void EyesHandler::setEyesState(int state) {
	_eyes_state.state;
}

void EyesHandler::setEyesPitchYaw(float pitch, float yaw) {
	_eyes_state.pitch = pitch;
	_eyes_state.yaw = yaw;
}

void EyesHandler::playEmotion() {
	if (!(_message_sent)) {
		std_msgs::Float64 left_msg;
		std_msgs::Float64 right_msg;
		std_msgs::Float64 pitch_msg;

		switch(_eyes_state.state) {
		case(EYES_SPIN): {
			// TODO Lol how to implement?
			break;
		}
		case(EYES_DOWN): {
			left_msg.data	= 0.0;
			right_msg.data	= 0.0;
			pitch_msg.data	= 0.8;
			break;
		}
		case(EYES_UP): {
			left_msg.data	= 0.0;
			right_msg.data	= 0.0;
			pitch_msg.data	= 0.0;
			break;
		}
		case(EYES_NEUTRAL): {
			left_msg.data	= 0.0;
			right_msg.data	= 0.0;
			pitch_msg.data	= 0.4;
			break;
		}
		default: {
			break;
		}
		}
		_eyes_left_pub.publish(left_msg);
		_eyes_right_pub.publish(right_msg);
		_eyes_pitch_pub.publish(pitch_msg);

		_message_sent = true;
	}

}

int main(int argc, char** argv) {
	ros::init(argc, argv, "eyesHandlerEmotion");

	EyesHandler handler;
	handler.init();
	handler.spin();
}
